#include "astra_camera.hpp"

#include <algorithm>
#include <string>

#include "PS1080.h"
#include "spdlog/spdlog.h"

Astra::Astra(/* args */) {}

Astra::~Astra() {}

ObCameraStatus Astra::DepthInit(void) {
  openni::Status rc = openni::OpenNI::initialize();
  if (rc != openni::STATUS_OK) {
    return CAMERA_STATUS_DEPTH_INIT_FAIL;
  }

  rc = device_.open(openni::ANY_DEVICE);
  if (rc != openni::STATUS_OK) {
    openni::OpenNI::shutdown();
    return CAMERA_STATUS_DEPTH_OPEN_FAIL;
  }

  depth_init_ = true;
  SPDLOG_INFO("Depth init");

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::DepthUnInit() {
  DepthStop();
  ColorStop();
  if (depth_init_) {
    device_.close();
    openni::OpenNI::shutdown();
  }

  depth_init_ = false;

  SPDLOG_INFO("depth deinit");
  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::DepthStart(int width, int height) {
  if (depth_start_) {
    return CAMERA_STATUS_SUCCESS;
  }

  openni::Status rc = openni::STATUS_OK;
  if (NULL == device_.getSensorInfo(openni::SENSOR_DEPTH)) {
    return CAMERA_STATUS_DEPTH_CREATE_FAIL;
  }

  if (!depth_stream_create_) {
    rc = depth_stream_.create(device_, openni::SENSOR_DEPTH);
    if (rc != openni::STATUS_OK) {
      return CAMERA_STATUS_DEPTH_CREATE_FAIL;
    }
  }

  depth_stream_create_ = true;
  switch (type_) {
    case HARDWARE_D2C:
      // hardware d2c must set d2c command
      device_.setImageRegistrationMode(
          openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
      break;
    case SOFTWARE_D2C:
      // soft d2c set depth not mirror
      depth_stream_.setMirroringEnabled(true);
      break;
    case NORMAL_NO:
      // Normal output
      break;
    default:
      SPDLOG_ERROR("d2c type is not support");
      break;
  }

  if (width > 0 && height > 0) {
    openni::VideoMode mode = depth_stream_.getVideoMode();
    mode.setResolution(width, height);
    depth_stream_.setVideoMode(mode);
  }

  rc = depth_stream_.start();
  if (rc != openni::STATUS_OK) {
    return CAMERA_STATUS_DEPTH_START_FAIL;
  }

  depth_start_ = true;

  SPDLOG_INFO("depth start ");

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::DepthStop() {
  if (depth_start_) {
    depth_stream_.stop();
  }

  depth_start_ = false;

  if (depth_stream_create_) {
    depth_stream_.destroy();
  }
  depth_stream_create_ = false;

  SPDLOG_INFO("depth stop ");

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::WaitDepthStream(openni::VideoFrameRef &frame) {
  int changedIndex = -1;

  openni::Status rc = openni::STATUS_OK;

  openni::VideoStream *streams[] = {&depth_stream_};

  rc = openni::OpenNI::waitForAnyStream(streams, 1, &changedIndex,
                                        DEPTH_TIMEOUT);
  if (rc == openni::STATUS_OK) {
    rc = depth_stream_.readFrame(&frame);
    if (rc != openni::STATUS_OK) {
      return CAMERA_STATUS_GET_DEPTH_FAIL;
    }
  } else {
    return CAMERA_STATUS_DEPTH_WAIT_TIMEOUT;
  }

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::CameraInit(int d2cType) {
  ObCameraStatus nRet = DepthInit();
  SetD2CType(d2cType);
  return nRet;
}

ObCameraStatus Astra::SetD2CType(int d2cType) {
  type_ = d2cType;
  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::CameraUnInit(void) {
  DepthUnInit();

  SPDLOG_INFO("CameraUnInit ");
  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::GetStreamData(cv::Mat &cv_rgb, cv::Mat &cv_depth) {
  // DWORD nStartTime = GetTickCount();
  ObCameraStatus nRet = ObCameraStatus::CAMERA_STATUS_SUCCESS;

  if (!depth_init_) {
    nRet = DepthInit();
    if (nRet != CAMERA_STATUS_SUCCESS) {
      return nRet;
    }
  }

  // depth start
  if (!depth_start_) {
    int nRet = GetCameraResolution(image_width_, image_height_);
    if (nRet != CAMERA_STATUS_SUCCESS) {
      return CAMERA_STATUS_DEPTH_GET_RESOLUTION_FAIL;
    }

    nRet = DepthStart(image_width_, image_height_);

    if (nRet != CAMERA_STATUS_SUCCESS) {
      return CAMERA_STATUS_DEPTH_START_FAIL;
    }
  }

  // color start
  if (!color_start_) {
    // rgb 分辨率为 640x480
    nRet = ColorStart(Orbbec::kL_640, Orbbec::kL_480);

    if (nRet != CAMERA_STATUS_SUCCESS) {
      return CAMERA_STATUS_DEPTH_START_FAIL;
    }
  }

  // wait depth stream
  openni::VideoFrameRef frame;
  nRet = WaitDepthStream(frame);

  if (nRet == CAMERA_STATUS_SUCCESS) {
    if (frame.getWidth() == image_width_ &&
        frame.getHeight() == image_height_) {
      int nFrameSize =
          sizeof(OniDepthPixel) * frame.getWidth() * frame.getHeight();
      memcpy(cv_depth.data, frame.getData(), nFrameSize);
      cv_depth.cols = frame.getWidth();
      cv_depth.rows = frame.getHeight();
    } else {
      return CAMERA_STATUS_GET_DEPTH_FAIL;
    }
  } else {
    return nRet;
  }

  // wait color stream
  openni::VideoFrameRef colorFrame;
  nRet = WaitColorStream(colorFrame);
  if (nRet == CAMERA_STATUS_SUCCESS) {
    if (colorFrame.getWidth() == Orbbec::kL_640 &&
        colorFrame.getHeight() == Orbbec::kL_480) {
      for (unsigned int y = 0; y < (Orbbec::kL_480); ++y) {
        uint8_t *data = (uint8_t *)cv_rgb.ptr<uchar>(y);
        for (unsigned int x = 0; x < Orbbec::kL_640; ++x) {
          // rgb888 to bgr888
          OniRGB888Pixel *streamPixel =
              (OniRGB888Pixel *)((char *)colorFrame.getData() +
                                 ((int)(y)*colorFrame.getStrideInBytes())) +
              (int)(x);
          data[0] = streamPixel->b;
          data[1] = streamPixel->g;
          data[2] = streamPixel->r;
          data = data + 3;
        }
      }
    } else {
      return CAMERA_STATUS_GET_RGB_FAIL;
    }
  } else {
    return nRet;
  }

  return nRet;
}

ObCameraStatus Astra::StreamStop(void) {
  DepthStop();
  ColorStop();

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::GetCameraParam(OBCameraParams &cameraParam) {
  int dataSize = sizeof(OBCameraParams);
  memset(&cameraParam, 0, sizeof(cameraParam));
  openni::Status rc = device_.getProperty(openni::OBEXTENSION_ID_CAM_PARAMS,
                                          (uint8_t *)&cameraParam, &dataSize);
  if (rc != openni::STATUS_OK) {
    return CAMERA_STATUS_DEPTH_GET_CAMERA_PARAM_FAIL;
  }

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::GetCameraResolution(int &nImageWidth, int &nImageHeight) {
  uint16_t pid = GetDevicePid();
  bool find = true;

  if (pid == Orbbec::kPID_0402) {
    image_width_ = Orbbec::kL_640;
    image_height_ = Orbbec::kL_480;
  } else {
    find = false;
  }

  if (find) {
    nImageWidth = image_width_;
    nImageHeight = image_height_;
    SPDLOG_DEBUG("image size (w, h) : {} , {}", image_width_, image_height_);
    return CAMERA_STATUS_SUCCESS;
  } else {
    return CAMERA_STATUS_DEPTH_GET_RESOLUTION_FAIL;
  }
}

uint16_t Astra::GetDevicePid(void) {
  uint16_t pid = device_.getDeviceInfo().getUsbProductId();
  SPDLOG_WARN("0x{0:04x}", pid);
  return pid;
}

ObCameraStatus Astra::ColorStart(int width, int height) {
  if (color_start_) {
    return CAMERA_STATUS_SUCCESS;
  }

  openni::Status rc = openni::STATUS_OK;
  if (NULL == device_.getSensorInfo(openni::SENSOR_COLOR)) {
    return CAMERA_STATUS_DEPTH_CREATE_FAIL;
  }

  if (!color_stream_create_) {
    rc = color_stream_.create(device_, openni::SENSOR_COLOR);
    if (rc != openni::STATUS_OK) {
      return CAMERA_STATUS_DEPTH_CREATE_FAIL;
    }
  }

  color_stream_create_ = true;

  if (width > 0 && height > 0) {
    openni::VideoMode mode = color_stream_.getVideoMode();
    mode.setResolution(width, height);
    color_stream_.setVideoMode(mode);
  }

  rc = color_stream_.start();
  if (rc != openni::STATUS_OK) {
    return CAMERA_STATUS_DEPTH_START_FAIL;
  }

  color_start_ = true;

  SPDLOG_INFO("color start ");

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::ColorStop() {
  if (color_start_) {
    color_stream_.stop();
  }

  color_start_ = false;

  if (color_stream_create_) {
    color_stream_.destroy();
  }

  color_stream_create_ = false;
  SPDLOG_INFO("color stop");

  return CAMERA_STATUS_SUCCESS;
}

ObCameraStatus Astra::WaitColorStream(openni::VideoFrameRef &frame) {
  int changedIndex = -1;

  openni::Status rc = openni::STATUS_OK;

  openni::VideoStream *streams[] = {&color_stream_};

  rc = openni::OpenNI::waitForAnyStream(streams, 1, &changedIndex,
                                        DEPTH_TIMEOUT);
  if (rc == openni::STATUS_OK) {
    rc = color_stream_.readFrame(&frame);
    if (rc != openni::STATUS_OK) {
      return CAMERA_STATUS_GET_RGB_FAIL;
    }
  } else {
    return CAMERA_STATUS_RGB_WAIT_TIMEOUT;
  }

  return CAMERA_STATUS_SUCCESS;
}

std::string StatusToString(ObCameraStatus event) {
  switch (event) {
    case ObCameraStatus::CAMERA_STATUS_SUCCESS:
      return std::string("CAMERA_STATUS_SUCCESS");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_INIT_FAIL:  // depth 初始化失败
      return std::string("CAMERA_STATUS_DEPTH_INIT_FAIL");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_OPEN_FAIL:  // depth open失败
      return std::string("CAMERA_STATUS_DEPTH_OPEN_FAIL");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_CREATE_FAIL:  // depth create流失败
      return std::string("CAMERA_STATUS_DEPTH_CREATE_FAIL");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_START_FAIL:  // depth start流失败
      return std::string("CAMERA_STATUS_DEPTH_START_FAIL");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_WAIT_TIMEOUT:  // depth等待流超时
      return std::string("CAMERA_STATUS_DEPTH_WAIT_TIMEOUT");
    case ObCameraStatus::CAMERA_STATUS_GET_DEPTH_FAIL:  // 获取depth流失败
      return std::string("CAMERA_STATUS_GET_DEPTH_FAIL");
    case ObCameraStatus::CAMERA_STATUS_GET_RGB_FAIL:  // 获取rgb流失败
      return std::string("CAMERA_STATUS_GET_RGB_FAIL");
    case ObCameraStatus::CAMERA_STATUS_RGB_INIT_FAIL:  // RGB初始化失败
      return std::string("CAMERA_STATUS_RGB_INIT_FAIL");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_DISCONNECT:  // depth 断开连接
      return std::string("CAMERA_STATUS_DEPTH_DISCONNECT");
    case ObCameraStatus::CAMERA_STATUS_DEVICE_NOT_FOUND:  // 未找到失败
      return std::string("CAMERA_STATUS_DEVICE_NOT_FOUND");
    case ObCameraStatus::CAMERA_STATUS_RESOLUTION_UPSUPPORT:  // 分辨率不支持
      return std::string("CAMERA_STATUS_RESOLUTION_UPSUPPORT");
    case ObCameraStatus::CAMERA_STATUS_DEPTH_GET_CAMERA_PARAM_FAIL:  // 内外参
      return std::string("CAMERA_STATUS_DEPTH_GET_CAMERA_PARAM_FAIL");
    case ObCameraStatus::
        CAMERA_STATUS_DEPTH_GET_RESOLUTION_FAIL:  // depth分辨率
      return std::string("CAMERA_STATUS_DEPTH_GET_RESOLUTION_FAIL");
    case ObCameraStatus::CAMERA_STATUS_RGB_CREATE_FAIL:  // color create流失败
      return std::string("CAMERA_STATUS_RGB_CREATE_FAIL");
    case ObCameraStatus::CAMERA_STATUS_RGB_START_FAIL:  // color start流失败
      return std::string("CAMERA_STATUS_RGB_START_FAIL");
    case ObCameraStatus::CAMERA_STATUS_RGB_WAIT_TIMEOUT:  // color 等待流超时
      return std::string("CAMERA_STATUS_RGB_WAIT_TIMEOUT");
    case ObCameraStatus::CAMERA_STATUS_IR_CREATE_FAIL:
      return std::string("CAMERA_STATUS_IR_CREATE_FAIL");
    case ObCameraStatus::CAMERA_STATUS_IR_START_FAIL:
      return std::string("CAMERA_STATUS_IR_START_FAIL");
    case ObCameraStatus::CAMERA_STATUS_IR_WAIT_TIMEOUT:
      return std::string("CAMERA_STATUS_IR_WAIT_TIMEOUT");
    case ObCameraStatus::CAMERA_STATUS_GET_IR_FAIL:
      return std::string("CAMERA_STATUS_GET_IR_FAIL");
    default:
      return std::string("CAMERA_STATUS_OTHER_FAIL");
  }
}

void WarnReport(ObCameraStatus event) {
  if (event != ObCameraStatus::CAMERA_STATUS_SUCCESS) {
    SPDLOG_ERROR("{}", StatusToString(event));
    assert(0);
  }
}

void WarnReport(bool event, const std::string &args) {
  if (!event) {
    SPDLOG_ERROR("{}", args);
    assert(0);
  }
}
